/*
 * Robot.h
 *
 *  Created on: Dec 7, 2023
 *      Author: Ikhyeon Cho
 *	 Institute: Korea Univ. ISR (Intelligent Systems & Robotics) Lab
 *       Email: tre0430@korea.ac.kr
 */

#ifndef FAKE_ODOM_ROBOT_H
#define FAKE_ODOM_ROBOT_H

#include <Eigen/Core>

class Robot
{
public:
  Robot() = default;
  void setPose2D(const Eigen::Vector3d& pose2D)
  {
    pose2D_ = pose2D;
  }
  const Eigen::Vector3d& getPose2D() const
  {
    return pose2D_;
  }

  void deadReckoning(const Eigen::Vector3d& velocity, double dt, double noise_pose, double noise_yaw)
  {
    const auto& vx = velocity.x();
    const auto& vy = velocity.y();
    const auto& w = velocity.z();
    
    Eigen::Vector3d delta_pose;
  
    delta_pose.x() = vx * dt;
    delta_pose.y() = vy * dt;
    delta_pose.z() = w * dt;
 
    double theta = pose2D_.z(); // 当前角度
    double cos_theta = std::cos(theta);
    double sin_theta = std::sin(theta);
 
    // 考虑当前姿态的旋转，计算旋转后的速度分量，并积分位置变化
    double delta_x_rotated = (vx * cos_theta - vy * sin_theta) * dt;
    double delta_y_rotated = (vx * sin_theta + vy * cos_theta) * dt;
 
    pose2D_.x() += delta_x_rotated + noise_pose;
    pose2D_.y() += delta_y_rotated + noise_pose;
    pose2D_.z() += delta_pose.z() + noise_yaw; 
  }

private:
  Eigen::Vector3d pose2D_{ 0, 0, 0 };
};

#endif